#include "driver_l1.h"
#include "drv_l1_sfr.h"
#include "arm.h"

#define INT_FIQ				0x40	// FIQ disable bit
#define INT_IRQ				0x80	// IRQ disable bit
#define INT_MASK			0xC0	// Interrupt disable mask (Both FIR and IRQ)
#define MODE_USR			0x10
#define MODE_FIQ			0x11
#define MODE_IRQ			0x12
#define MODE_SVC			0x13
#define MODE_ABT			0x17
#define MODE_UND			0x1B
#define MODE_SYS			0x1F
#define MODE_MASK			0x1F
#define INSTRUCTION_THUMB	0x20

extern INT32U	pHandler_Undef;
extern INT32U	pHandler_SWI;
extern INT32U	pHandler_PAbort;
extern INT32U	pHandler_DAbort;
extern INT32U	pHandler_IRQ;
extern INT32U	pHandler_FIQ;

void debug_undefined(INT32U address)
{
	DBG_PRINT("%s(%x)\r\n", __func__, address);
	while(1);
}

void debug_swi(INT32U address)
{
	DBG_PRINT("%s(%x)\r\n", __func__, address);
	while(1);
}

void debug_prefetch_abort(INT32U address)
{
	DBG_PRINT("%s(%x)\r\n", __func__, address);
	while(1);
}

void debug_data_abort(INT32U address)
{
	DBG_PRINT("%s(%x)\r\n", __func__, address);
	while(1);
}

void debug_irq(void)
{
	DBG_PRINT("%s\r\n", __func__);
	while(1);
}

void debug_fiq(void)
{
	DBG_PRINT("%s\r\n", __func__);
	while(1);
}

void exception_table_init(void)
{
	pHandler_Undef = (INT32U) debug_undefined;
	pHandler_SWI = (INT32U) debug_swi;
	pHandler_PAbort = (INT32U) debug_prefetch_abort;
	pHandler_DAbort = (INT32U) debug_data_abort;
	pHandler_IRQ = (INT32U) debug_irq;
	pHandler_FIQ = (INT32U) debug_fiq;
}

INT32S register_exception_table(INT32U exception, INT32U handler)
{
	switch(exception)
	{
	case EXCEPTION_UNDEF:
		pHandler_Undef = (INT32U) handler;
		break;

	case EXCEPTION_SWI:
		pHandler_SWI = (INT32U) handler;
		break;

	case EXCEPTION_PABORT:
		pHandler_PAbort = (INT32U) handler;
		break;

	case EXCEPTION_DABORT:
		pHandler_DAbort = (INT32U) handler;
		break;

	case EXCEPTION_IRQ:
		pHandler_IRQ = (INT32U) handler;
		break;

	case EXCEPTION_FIQ:
		pHandler_FIQ = (INT32U) handler;
		break;

	default:
		return STATUS_FAIL;
	}

	return STATUS_OK;
}

#if _OPERATING_SYSTEM == _OS_NONE	// Hard protect should not be used when OS is available
void irq_enable(void)
{
	int x;
	__asm
	{
		MRS x, CPSR
	};
	__asm
	{
		BIC x, x, #INT_IRQ
	};
	__asm
	{
		MSR CPSR_c, x
	};
}

void irq_disable(void)
{
	int x;
	__asm
	{
		MRS x, CPSR
	};
	__asm
	{
		ORR x, x, #INT_IRQ
	};
	__asm
	{
		MSR CPSR_c, x
	};
}

void fiq_enable(void)
{
	int x;
	__asm
	{
		MRS x, CPSR
	};
	__asm
	{
		BIC x, x, #INT_FIQ
	};
	__asm
	{
		MSR CPSR_c, x
	};
}

void fiq_disable(void)
{
	int x;
	__asm
	{
		MRS x, CPSR
	};
	__asm
	{
		ORR x, x, #INT_FIQ
	};
	__asm
	{
		MSR CPSR_c, x
	};
}

#endif
